/*
 * Plane.cpp
 *
 *  Created on: Apr 29, 2009
 *      Author: wrice
 */
#include "MyVideoTrace.h"
namespace MyVideoTrace {

Plane::Plane( Vector3f normal, Point3D aPointOnPlane )
: normal_(normal.normalized())
,aPointOnPlane_(aPointOnPlane)
{
}

float Plane::getD() const {
	const Vector3f aPointOnPlane(aPointOnPlane_.getX(), aPointOnPlane_.getY(), aPointOnPlane_.getZ());
	return - (normal_.dotProduct( aPointOnPlane ));
}

float Plane::distance( Point3D aPoint ) const {
	// http://en.wikipedia.org/wiki/Plane_(mathematics)#Distance_from_a_point_to_a_plane
	const float a = normal_.getX();
	const float b = normal_.getY();
	const float c = normal_.getZ();
	const float d = getD();
	const float x1 = aPoint.getX();
	const float y1 = aPoint.getY();
	const float z1 = aPoint.getZ();
	const float dinominator = sqrt( a * a + b * b + c + c);
	const float distance = (a * x1 + b * y1 + c * z1 + d) / dinominator;
	return (distance >= 0) ? distance : -distance;
}

}
